#include "dsp.h"

float32_t lo_i[ADC_SIZE];
float32_t lo_q[ADC_SIZE];

void compute_lo(uint32_t freq)
{
    float32_t k = 2 * PI * freq / SAMPLING_RATE;
    for (int i = 0; i < ADC_SIZE; i++)
    {
        lo_i[i] = arm_cos_f32(i * k);
        lo_q[i] = -arm_sin_f32(i * k);
    }
}

void mix_sig(float32_t *sig,float32_t *sig_i, float32_t *sig_q)
{
    arm_mult_f32(sig, lo_i, sig_i, ADC_SIZE);
    arm_mult_f32(sig, lo_q, sig_q, ADC_SIZE);
}

void sum_filter(float32_t *sig,float32_t *sig_out)
{
    uint16_t dec=DEC_FACTOR,j=0;
    float32_t sum=0.f;
    for(uint16_t i=0;i<ADC_SIZE;i++)
    {
        sum+=sig[i];
        if((i+1)%dec==0)
        {
            sig_out[j++]=sum;
            sum=0.f;
        }
    }
}

void demod_am(float32_t *sig_i, float32_t *sig_q, float32_t *audio,uint16_t blockSize)
{
    float32_t sig_iq[blockSize * 2];
    for (int n = 0; n < blockSize; n++)
    {
        sig_iq[2 * n + 0] = sig_i[n];
        sig_iq[2 * n + 1] = sig_q[n];
    }
    arm_cmplx_mag_f32(sig_iq, audio, blockSize);
}

void int2float(uint16_t *i_data,float32_t *f_data)
{
    for(int i=0;i<ADC_SIZE;i++)
    {
        f_data[i]=(i_data[i]-2048)/2048.f;
    }
}
void float2int(float32_t *f_data,uint16_t *i_data)
{
    float32_t vol=0.04;
    for(int i=0;i<DAC_SIZE;i++)
    {
        i_data[i]=f_data[i]*2048.f*vol+2048.f;
    }
}

void process_sig(uint16_t *adc_value,uint16_t *dac_value)
{
    // 1.int2float
    float32_t sig[ADC_SIZE];
    int2float(adc_value,sig);

    // // 2.mix
    float32_t sig_i[ADC_SIZE];
    float32_t sig_q[ADC_SIZE];
    mix_sig(sig,sig_i,sig_q);

    // 3.cic_decimate
    float32_t sig_i_dec[DAC_SIZE];
    float32_t sig_q_dec[DAC_SIZE];
    sum_filter(sig_i,sig_i_dec);
    sum_filter(sig_q,sig_q_dec);

    // 4.demod
    float32_t audio[DAC_SIZE];
    demod_am(sig_i_dec,sig_q_dec,audio,DAC_SIZE);

    // 5. float转int
    float2int(audio,dac_value);
}